from time import sleep
from .servo_gpiozero import Servo

class Dual_Axis:
    LEFT=0
    RIGHT=1
    UP=2
    DOWN=3
    CENTER=4
    OPENCV_LANE=5

    def __init__(self,horizontal_Pin,horizontal_init_angle,vertical_Pin,vertical_init_angle):
        self.horizontal_servo=Servo(horizontal_Pin,horizontal_init_angle)
        self.vertical_servo=Servo(vertical_Pin,vertical_init_angle) 
        
        self.DICT_CMD_ACTION = {Dual_Axis.LEFT: self.turn_left,
                                Dual_Axis.RIGHT: self.turn_right,
                                Dual_Axis.UP: self.turn_up,
                                Dual_Axis.DOWN: self.turn_down,
                                Dual_Axis.CENTER: self.turn_center,
                                Dual_Axis.OPENCV_LANE: self.turn_opencv_lane
    
                                }        
        self.each_step_angle=5

    def turn_left(self):
        self.horizontal_servo.turn_clockwise(self.each_step_angle)
    
    def turn_right(self):
        self.horizontal_servo.turn_anti_clockwise(self.each_step_angle)

    def turn_up(self):
        self.vertical_servo.turn_anti_clockwise(self.each_step_angle)
    
    def turn_down(self):
        self.vertical_servo.turn_clockwise(self.each_step_angle)
    
    def turn_center(self):
        self.horizontal_servo.turn_to_angle(90)
        self.vertical_servo.turn_to_angle(75)
   
    def turn_opencv_lane(self):
        self.horizontal_servo.turn_to_angle(90)
        self.vertical_servo.turn_to_angle(5)
   
    def action(self, cmd_to_dual_axis):
        self.DICT_CMD_ACTION[cmd_to_dual_axis]()

if  __name__=="__main__":
    import sys
    sys.path.append('/home/cmh/RaspberryPi_Ground/pi-vision-car/webapp/components/')
    dual_axis=Dual_Axis(18,90,19,90)
    dual_axis.turn_left()
    sleep(1)
    dual_axis.turn_right()
    sleep(1)
    dual_axis.turn_left()
    sleep(1)
    dual_axis.turn_up()
    sleep(1)
    dual_axis.turn_down()
    sleep(1)
    dual_axis.turn_up()
    sleep(1)
   
    